import numpy as np
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from collections import deque


def convert_lidar_scan_to_local(msg: LaserScan):
    """
    Convert Laser scan to distances, angles and information
    """
    distances = np.array([])
    angles = np.array([])
    information = np.array([])

    for i in range(len(msg.ranges)):
        ang = i * msg.angle_increment
        dist = -1.0
        if msg.ranges[i] > msg.range_max:
            dist = msg.range_max
        elif msg.ranges[i] < msg.range_min:
            dist = msg.range_min
        else:
            dist = msg.ranges[i]
        infor = ((msg.range_max - dist) / msg.range_max) ** 2

        distances = np.append(distances, dist)
        angles = np.append(angles, ang)
        information = np.append(information, infor)

    return distances, angles, information


def convert_odom(msg: Odometry):
    """
    Convert Odometry to angles and translations
    return x, y, theta
    """
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    quat = msg.pose.pose.orientation
    quat = [quat.x, quat.y, quat.z, quat.w]
    (r,p,yaw) = euler_from_quaternion(quat)
    if yaw < 0:
        yaw += 2 * np.pi
    return x, y, yaw


def robotic_2_inertial(odom_x, odom_y, theta, distances, angles):
    """
    Transform the result of a lidar scan to the inertial frame
    """
    x, y = np.array([]), np.array([])
    for (dist, ang) in zip(distances, angles):
        x = np.append(x, odom_x + dist * np.cos(ang + theta))
        y = np.append(y, odom_y + dist * np.sin(ang + theta))
    return x, y


class MessageHandler(object):
    def __init__(self):
        self.scanMsgStack = deque()
        self.odomMsgStack = deque()

    def accept_message(self, topic, msg):
        """
        Receive a message
        """
        if topic == "/scan":
            self.scanMsgStack.append(msg)
        elif topic == "/odom":
            self.odomMsgStack.append(msg)

    def pair_check(self):
        """
        Check if /odom and /scan messages can be paired
        """
        if len(self.scanMsgStack) == 0 or len(self.odomMsgStack) == 0:
            return False
        else:
            return True

    def process_paired_messages(self):
        """
        Return paired odom and laserscan messages
        """
        msgOdom = self.odomMsgStack.pop()
        msgScan = self.scanMsgStack.pop()
        return msgScan, msgOdom